﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Hook.Environmental.Props;
using Microsoft.Xna.Framework;

namespace Hook.Environmental
{
    public class StretchChain
    {
        /*public const float RESTSPRING = 0.055f;
        public const float RESTDAMPING = 0.175f;
        public const float CONTRACTSPRING = 0.225f;
        public const float CONTRACTDAMPING = 0.055f;
        public const float EXTENDSPRING = 0.0075f;
        public const float EXTENDDAMPING = 0.055f;
        public const float STICKSPRING = 0.002f;
        public const float STICKDAMPING = 0.055f;*/     //quadratic values

        public const float RESTSPRING = 0.5f;
        public const float RESTDAMPING = 0.075f;
        public const float CONTRACTSPRING = 0.8f;
        public const float CONTRACTDAMPING = 0.1f;
        public const float EXTENDSPRING = 0.055f;
        public const float EXTENDDAMPING = 0.055f;
        public const float STICKSPRING = 0.35f;
        public const float STICKDAMPING = 0.075f;       //linear values

        public const float RETRACTFACTOR = 2.5f;
        public const float RETRACTFLOOR = 0.0f;

        //public const float DISTANCE

        public const int REST = 0;
        public const int CONTRACT = 1;
        public const int EXTEND = 2;
        public const int STICK = 3;

        public List<Prop> Props;
        public int ChainState;
        public float RetractSpeed;
        public float SpringFactor, DampingFactor;
        public float SpringDistanceFactor;

        public StretchChain()
        {
            Props = new List<Prop>();
            SpringDistanceFactor = 1;
            Rest();
        }

        public void Rest()
        {
            ChainState = REST;
            SpringFactor = RESTSPRING;
            DampingFactor = RESTDAMPING;
        }
        public void Contract()
        {
            ChainState = CONTRACT;
            SpringFactor = CONTRACTSPRING;
            DampingFactor = CONTRACTDAMPING;
            Props[0].CollisionLayer.Clear();
            ((ChainHead)Props[0]).UnRoot();
        }
        public void Extend()
        {
            ChainState = EXTEND;
            SpringFactor = EXTENDSPRING;
            DampingFactor = EXTENDDAMPING;
            Props[0].CollisionLayer.Add(1);
        }
        public void Stick(Prop P)
        {
            ChainState = STICK;
            SpringFactor = STICKSPRING;
            DampingFactor = STICKDAMPING;
            ((ChainHead)Props[0]).Root(P);
        }

        public bool CanExtend()
        {
            if (ChainState == REST || ChainState == CONTRACT)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        public bool CanContract()
        {
            if (ChainState != REST)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public void ApplySpringForce()
        {
            Vector3 Direction;
            float Distance;

            for (int p = 0; p < Props.Count - 1; p++)
            {
                Direction = Props[p + 1].Position - Props[p].Position;
                Distance = Direction.Length();
                if (Distance != 0)
                    Direction.Normalize();

                //Set joint angles
                if (p + 1 != Props.Count - 1)
                {
                    float Angle = (float)Math.Atan2(Direction.Y, Direction.X);
                    Angle -= (float)Math.PI / 2;
                    Props[p + 1].Rotation.Z = Angle;
                }

                if (ChainState == EXTEND || ChainState == STICK)
                {
                    Props[p].QueueForce(SpringFactor * Distance * Direction);
                    if (!(Props[p + 1] is Player) || ChainState == EXTEND || ChainState == STICK)
                        Props[p + 1].QueueForce(SpringFactor * Distance * -Direction);
                }
                else if (ChainState == REST)
                {
                    Props[0].SetPosition(Props[Props.Count - 1].Position);
                }
                else
                {
                    Props[p].QueuedForce = Vector3.Zero;
                    RetractSpeed = RETRACTFLOOR + RETRACTFACTOR * Distance; //(float)Math.Pow(Distance, 1.5);
                    Props[p].Velocity = RetractSpeed * Direction;
                }
            }
        }

        public void ApplySpringDamping()
        {
            Vector3 Distance, Vel1, Vel2, RelativeVelocity;
            if (ChainState == EXTEND || ChainState == STICK)
            {
                for (int p = 0; p < Props.Count - 1; p++)
                {
                    Distance = Props[p + 1].Position - Props[p].Position;
                    Vel1 = Prop.Projection(Distance, Props[p].Velocity);
                    Vel2 = Prop.Projection(Distance, Props[p + 1].Velocity);
                    RelativeVelocity = Vel1 - Vel2;

                    Props[p].QueueForce(DampingFactor * -RelativeVelocity);
                    Props[p + 1].QueueForce(DampingFactor * RelativeVelocity);
                }
            }
        }

        public void UpdateVelocities()
        {
            foreach (Prop P in Props)
            {
                P.ApplyForceAndTorque();
            }
        }

        public void PositionChain(Vector3 Position)
        {
            for (int p = 0; p < Props.Count - 1; p++)
            {
                Props[p].SetPosition(Position);
                Props[p].Velocity = Vector3.Zero;
                Props[p].QueuedForce = Vector3.Zero;
            }
        }

        public void AddJoints(List<Prop> StretchJoints)
        {
            Prop TempProp = Props[Props.Count - 1];
            Props.RemoveAt(Props.Count - 1);
            Props.AddRange(StretchJoints);
            Props.Add(TempProp);
        }

        public void AddJoint(Prop StretchJoint)
        {
            Prop TempProp = Props[Props.Count - 1];
            Props.RemoveAt(Props.Count - 1);
            Props.Add(StretchJoint);
            Props.Add(TempProp);
        }

        public Prop RemoveLastJoint()
        {
            Prop RemovedProp = null;
            if (Props.Count > 2)
            {
                RemovedProp = Props[Props.Count - 2];
                Props.RemoveAt(Props.Count - 2);
            }
            return RemovedProp;
        }

        public List<Prop> RemoveAllJoints()
        {
            List<Prop> RemovedProps = new List<Prop>();
            while (Props.Count > 2)
            {
                RemovedProps.Add(Props[Props.Count - 2]);
                Props.RemoveAt(Props.Count - 2);
            }
            return RemovedProps;
        }
    }
}
